#include <cv.h>
#include <highgui.h>
#include <ctype.h>
#include <stdio.h>
#include <stdlib.h>
#define PI 3.14159265

#ifndef MAX
  #define MAX(a, b) (((a) > (b)) ? (a) : (b))
#endif

cv::Mat mCameraFrame;
int mFrameHeight = 480;
int mFrameWidth = 640;
int mFilterBoxSize = 5;
enum EObstructionStatus
{
  EObstructionStatusNoObject,
  EObstructionStatusObjectDetected
};


int
DetectObstruction( EObstructionStatus& rStatus, double& rDistance, double& rAngle )
{
  // HSV Colors are used for object detection 
  cv::Mat HsvCameraFrame;
  cv::Mat BinaryFrame(   mFrameHeight, mFrameWidth, CV_8UC1 );
  cv::Mat FilteredFrame( mFrameHeight, mFrameWidth, CV_8UC1 );

  // Convert the camera frame
  cv::cvtColor( mCameraFrame, HsvCameraFrame, CV_BGR2HSV );

  // Convert HSV camera frame to binary image ( extract blue )
  for( unsigned i = 0; i < mFrameHeight; i++ )
  {
    for( unsigned j = 0; j < mFrameWidth; j++ )
    {
      if( ( ( HsvCameraFrame.data[ ( i * mFrameWidth + j ) * 3 ] > 105 )     && 
        (     HsvCameraFrame.data[ ( i * mFrameWidth + j ) * 3 ] < 120 )     && 
        (     HsvCameraFrame.data[ ( i * mFrameWidth + j ) * 3 + 1 ] > 40 )  && 
        (     HsvCameraFrame.data[ ( i * mFrameWidth + j ) * 3 + 2 ] > 40 ) ) )
      {
        BinaryFrame.data[ ( i * mFrameWidth + j ) ] = 255;
      }
      else
      {
        BinaryFrame.data[ ( i * mFrameWidth + j ) ] = 0;
      }
    }
  }

  // Apply box filter to the binary image 
  // Determine extremes / edges 
  unsigned MaxX = 0, MinX = mFrameWidth;
  unsigned MaxY = 0, MinY = mFrameHeight;
  for( unsigned i = mFilterBoxSize; i < ( mFrameHeight - mFilterBoxSize ); i++ )
  {
    for( unsigned j = mFilterBoxSize; j < ( mFrameWidth - mFilterBoxSize ); j++ )
    {
      unsigned WhitePixelsInArea = 0;
      for( int k = -mFilterBoxSize; k <= mFilterBoxSize; k++ )
      {
        for( int l = -mFilterBoxSize; l <= mFilterBoxSize; l++ )
        {
          if( 255 == BinaryFrame.data[ ( ( i + k ) * mFrameWidth ) + j + l ] )
          {
            WhitePixelsInArea++;
          }
        }
      }

      // Pixed threshold in the box area
      if( WhitePixelsInArea > 110 )
      {
        FilteredFrame.data[ ( i * mFrameWidth ) + j ] = 255;
        MaxX = ( ( j > MaxX ) ? j : MaxX );
        MinX = ( ( j < MinX ) ? j : MinX );
        MaxY = ( ( i > MaxY ) ? i : MaxY );
        MinY = ( ( i < MinY ) ? i : MinY );
      }
      else
      {
        FilteredFrame.data[ ( i * mFrameWidth ) + j ] = 0;
      }
    }
  }
  
  // Set obstruction status
  rStatus = ( -mFrameWidth == MaxX - MinX ) 
    ? EObstructionStatusNoObject 
    : EObstructionStatusObjectDetected;

  // If object is detected, draw box
  if( EObstructionStatusObjectDetected == rStatus )
  {
    for(unsigned  i = MinX; i <= MaxX; i++ )
    {
      mCameraFrame.data[ ( mFrameWidth * MinY + i ) * 3 ]      = 0;
      mCameraFrame.data[ ( mFrameWidth * MinY + i ) * 3 + 1 ]  = 255;
      mCameraFrame.data[ ( mFrameWidth * MinY + i ) * 3 + 2 ]  = 0;
      mCameraFrame.data[ ( mFrameWidth * MaxY + i ) * 3 ]      = 0;
      mCameraFrame.data[ ( mFrameWidth * MaxY + i ) * 3 + 1 ]  = 255;
      mCameraFrame.data[ ( mFrameWidth * MaxY + i ) * 3 + 2 ]  = 0;
    }
    for(unsigned  i = MinY; i <= MaxY; i++ )
    {
      mCameraFrame.data[ ( mFrameWidth * i + MinX ) * 3 ]      = 0;
      mCameraFrame.data[ ( mFrameWidth * i + MinX ) * 3 + 1 ]  = 255;
      mCameraFrame.data[ ( mFrameWidth * i + MinX ) * 3 + 2 ]  = 0;
      mCameraFrame.data[ ( mFrameWidth * i + MaxX ) * 3 ]      = 0;
      mCameraFrame.data[ ( mFrameWidth * i + MaxX ) * 3 + 1 ]  = 255;
      mCameraFrame.data[ ( mFrameWidth * i + MaxX ) * 3 + 2 ]  = 0;
    }	
  }

  // If the object is in the center it's easy, assume it is
  int       ObstructionWidth  = MAX( MaxX - MinX, MaxY - MinY );
  unsigned  ObstructionX      = ( MaxX + MinX ) / 2;
  unsigned  ObstructionY      = ( MaxY + MinY ) / 2;
  if( EObstructionStatusObjectDetected == rStatus )
  {
    // Determine Distance and angle with this information
    printf( "  Obstruction width:  %d\n", ObstructionWidth );
    printf( "  Obstruction center: %d,%d\n", ObstructionX, ObstructionY );
  
    // Set the real distance 
    if( ObstructionWidth < 200 )
    {
      rDistance = 60 + ( static_cast<double>( ObstructionWidth ) * -0.2 );
    }
    else
    {
      rDistance = 30 + ( static_cast<double>( ObstructionWidth ) * -0.045 );
    }

    // Set the angle
    double LHS = ( ( static_cast<double>( ObstructionX ) - 320) / 22 );
    double RHS = ( 30 - rDistance ) / 3.5;
    double Variable = 0.0;

    if( ObstructionX > 320 )
    {
      Variable = LHS - RHS;
    }
    else
    {
      Variable = LHS + RHS;
    }
      rAngle = atan( Variable / rDistance ) * 180.0 / PI;
    }
}

int 
main( int argc, char** argv )
{
	cv::VideoCapture cap( 0 );
  bool Run = true;

  if( 1 != argc &&  2 != argc && 3 != argc )
  {
    printf( "Usage: %s [files/image.png] [halt]\n", argv[0] );
    printf( " No parameters: use web-cam, no halt parameter requires key-press\n" );
    return -1;
  }
 
  if( 1 == argc )
  {
    if( !cap.isOpened() )
    {  
      // check if we succeeded
		  printf("Failed to open capture device\n");
      return -1;
    }
  }
  else
  {
    mCameraFrame = cv::imread( argv[1], 3 ); // webcam
    printf( "Image: %s\n", argv[1] );
	}

  while( Run )
  {

    if( 1 == argc )
    {
	    cap >> mCameraFrame;
    }
    EObstructionStatus Status;
    double Distance = 0.0, Angle = 0.0;
    DetectObstruction( Status, Distance, Angle );
    printf("  Obstruction detected: %s\n", ( EObstructionStatusObjectDetected == Status ? "true" : "false" ) );
    printf("  Obstruction distance: %.2f[cm], angle: %.2f[deg]\n", Distance, Angle );
    cv::imshow("Image", mCameraFrame );
    
    if( 2 == argc || 3 == argc )
    {
      Run = false;
    }
    
    if( 1 == argc || 3 == argc )
    {
      if(cv::waitKey(5) >= 0) 
      {
        break;
      }
    }
  }


#if 0
  
	printf("*********************************** \n");
	printf("*************starting************** \n");
	//image is saved as BGR
	image = cv::imread( argv[1], 3 ); // webcam
	cv::namedWindow( "Image",  1 );
	cv::namedWindow( "Image_out",  1 );
	int xsize=image.cols, ysize=image.rows;
	printf("xsize %d, ysize %d \n", xsize, ysize);
	cv::Mat image_out(image.rows, image.cols, CV_8UC1); // black-white representation of the blue pixels in the image
	cv::Mat image_out2(image.rows, image.cols, CV_8UC1); // black-white after filtering 
	cv::Mat image_HSV; // used for internal filtering

	cv::cvtColor(image, image_HSV, CV_BGR2HSV);
	for(int i=0; i<ysize;i++){
		for(int j=0; j<xsize; j++){
			// detect if pixel is blue and has a certain intensity (in S and V)
			if(((image_HSV.data[(i*xsize+j)*3]>105) && (image_HSV.data[(i*xsize+j)*3]<120) && (image_HSV.data[(i*xsize+j)*3+1]>80) && (image_HSV.data[(i*xsize+j)*3+2]>80))){
				image_out.data[(i*xsize+j)]=255;
			}
			else{
				image_out.data[(i*xsize+j)]=0;
			}
		}
	}

	// apply boxfilter and determine extremes
	int max_x=0, min_x=xsize;
	int max_y=0, min_y=ysize;
	// real boxsize is boxsize*2+1
	int boxsize=4;
	for(int i=boxsize; i<(ysize-boxsize);i++){
		for(int j=boxsize; j<(xsize-boxsize); j++){
			//count nr of white pixels aroud this pixel
			int wittepixels=0;
			for(int k=-boxsize;k<=boxsize;k++){
				for(int l=-boxsize;l<=boxsize;l++){
					if(image_out.data[((i+k)*xsize)+j+l]==255){
						wittepixels++;
					}
				}
			}
			if(wittepixels>75){
				image_out2.data[(i*xsize)+j]=255;
				if(j>max_x){max_x=j;}
				if(j<min_x){min_x=j;}
				if(i>max_y){max_y=i;}
				if(i<min_y){min_y=i;}
			}
			else{
				image_out2.data[(i*xsize)+j]=0;				
			}
		}
	}
	// print edges of the ball
	printf("min_x: %d, max_x: %d, min_y: %d, max_y: %d\n", min_x, max_x, min_y, max_y);
	// print region of the ball
	printf("size of the ball (x,y): %d, %d \n",(max_x-min_x),(max_y-min_y) );
	// draw boudingbox in original image
	for (int i=min_x; i<=max_x; i++){
		image.data[(xsize*min_y+i)*3]=0;
		image.data[(xsize*min_y+i)*3+1]=255;
		image.data[(xsize*min_y+i)*3+2]=0;
		image.data[(xsize*max_y+i)*3]=0;
		image.data[(xsize*max_y+i)*3+1]=255;
		image.data[(xsize*max_y+i)*3+2]=0;
	}
	for (int i=min_y; i<=max_y; i++){
		image.data[(xsize*i+min_x)*3]=0;
		image.data[(xsize*i+min_x)*3+1]=255;
		image.data[(xsize*i+min_x)*3+2]=0;
		image.data[(xsize*i+max_x)*3]=0;
		image.data[(xsize*i+max_x)*3+1]=255;
		image.data[(xsize*i+max_x)*3+2]=0;
	}	
	int avg_x=(int)((max_x+min_x)/2);
	int avg_y=(int)((max_y+min_y)/2);
	printf("center pixel %d, %d \n", avg_x, avg_y);
	// draw center pixels
	for(int i=-3; i<=3; i++){
		for(int j=-3; j<=3; j++){
			image.data[(((avg_y+i)*xsize)+avg_x+j)*3]=0;
			image.data[(((avg_y+i)*xsize)+avg_x+j)*3+1]=0;
			image.data[(((avg_y+i)*xsize)+avg_x+j)*3+2]=255;
		}
	}
	cv::imshow("Image", image );
	cv::imshow("Image_out", image_out2 );
	cv::waitKey(0);
	printf("***********finished**************** \n");
	printf("*********************************** \n");
#endif 
}
